Generic Drake/ROS systems + RS Flip Flop example #6

sloretz · 2020-11-14T00:48:14Z

This adds generic ROS publisher/subscriber systems as described in RobotLocomotion/drake#9500. It's similar to https://github.com/gizatt/drake_ros_systems , but this uses ROS 2 and matches the LCM publisher/subscriber system's design more closely.

RS Flip Flop example

There's an example of an RS flip flop system that listens to topics /S, and /R and publishes to topics /Q and /Q_not.

To try this example out:

Install ROS Rolling - it might work with other ROS 2 versions, but I haven't tried it
Clone this repo into a colcon workspace
colcon build --packages-select drake_ros_systems
Run one of the rs_flip_flop examples
- C++ from the build folder: ./build/drake_ros_systems/example/rs_flip_flop
- Python from the example folder: python3 rs_flip_flop.py
Play with it with some of these commands:

Echo output topics:

$ ros2 topic echo /Q

ros2 topic echo /Q_not

Send values to the input topics

$ ros2 topic pub /S std_msgs/msg/Bool "data: false"
$ ros2 topic pub /S std_msgs/msg/Bool "data: true"

$ ros2 topic pub /R std_msgs/msg/Bool "data: false"
$ ros2 topic pub /R std_msgs/msg/Bool "data: true"

Future enhancements

C++

Allow picking the executor and executor options in DrakeRos
Add return value to spin functions in rclcpp that indicate f if spin did something, and if so pause Drake's concept of time to handle multiple messages just like LcmInterfaceSystem does.
Replace Publisher and Subscriber classes with Generic Publisher and Subscriber being worked on in Add rclcpp_generic, a publisher and subscription for serialized messages ros2/rclcpp#1452

Python

Use pybind11::capsule::get_pointer() when available

Code copying

The generic publisher and subscriber using raw messages was significantly copied from rosbag2_transport: https://github.com/ros2/rosbag2/tree/master/rosbag2_transport but it should probably be change to use these classes (not yet merged ros2/rclcpp#1452)

The Drake boilerplate in RosInterfaceSystem, RosSubscriberSystem, and RosPublisherSystem was significantly copied from the equivalent LCM systems in drake.

This change is

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

drake_ros_systems/README.md

sloretz · 2020-11-17T17:13:38Z

@IanTheEngineer @EricCousineau-TRI @azeey FYI

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

drake_ros_systems/include/drake_ros_systems/drake_ros.hpp

Publisher and ROS interface working, subscriber not yet working. Signed-off-by: Shane Loretz <sloretz@openrobotics.org> Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

azeey

LGTM! However, I don't understand much of the code in py_serializer.hpp, so another pair of eyes would be welcome.

I have built and tested the example with both the colcon and cmake workflows, and they both work!

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

This adds a way to provide the node name and node options, as well as a way to externally manage init/shutdown on a context. If the given node options have a valid context, then DrakeRos won't call init or shutdown on it. In this way external code can pass init options. Signed-off-by: Shane Loretz <sloretz@openrobotics.org> Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Adds half a type caster (Python -> C++) for rclpy.qos.QoSProfile to rclcpp::QoS. Adds QoS arguments to Python publisher and subscriber systems. Updates the Python example to pass QoS. Signed-off-by: Shane Loretz <sloretz@openrobotics.org> Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

EricCousineau-TRI · 2020-12-17T17:52:10Z

LGTM! However, I don't understand much of the code in py_serializer.hpp, so another pair of eyes would be welcome.

Placing one pair of 👀 here

EricCousineau-TRI · 2020-12-17T17:52:30Z

(Going to use Reviewable tho)

EricCousineau-TRI

Reviewable status: 0 of 25 files reviewed, 3 unresolved discussions (waiting on @sloretz)

drake_ros_systems/src/python_bindings/py_serializer.hpp, line 30 at r4 (raw file):

{
// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage
class PySerializer : public SerializerInterface

It's a bit challenging to analyze and review Python code flow in C++.
(Also, pumping stuff back and forth btw the two may be less efficient?)

As my own rule of thumb, anything that calls pybind11s Python C++ API more than 3 times in succession should generally be done in a Python function.

Is it possible for you to relegate this code to Python, and use something like what Drake to does to invoke Python code?

Example:
https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/pydrake_pybind.h#L127

https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/__init__.py#L77-L80

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

sloretz

Reviewable status: 0 of 27 files reviewed, 3 unresolved discussions (waiting on @EricCousineau-TRI and @sloretz)

drake_ros_systems/src/python_bindings/py_serializer.hpp, line 30 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

It's a bit challenging to analyze and review Python code flow in C++.
(Also, pumping stuff back and forth btw the two may be less efficient?)

As my own rule of thumb, anything that calls pybind11s Python C++ API more than 3 times in succession should generally be done in a Python function.

Is it possible for you to relegate this code to Python, and use something like what Drake to does to invoke Python code?

Example:
https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/pydrake_pybind.h#L127

https://github.com/RobotLocomotion/drake/blob/2baad6f34b12bfea1c583dfd63b0970b7eeb479a/bindings/pydrake/__init__.py#L77-L80

Relegated a bit more to Python in 22880a8

sloretz · 2020-12-18T22:37:25Z

I think this PR is just waiting on review. There's a little bit of test coverage now in the form of linters, a couple integration tests (one for C++, one for Python) and a small unit test for the DrakeRos class.

sloretz · 2021-09-08T16:28:40Z

Most of this is going into https://github.com/RobotLocomotion/drake-ros

I'll merge this and archive this repo.

sloretz added 10 commits November 9, 2020 17:03

Stub code builds

25b6b29

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Misc fixes to get it to build (but not link) with stub example

beb8c7d

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Fix comments on publisher system

3c7dd28

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

ROS interface implementation fleshed out

84b91a3

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Possible RosInterfaceSystem impl

7cebeec

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Possible RosSubscriberSystem impl

7b3a07c

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Possible RosPublisherSystem implementation

e3bc643

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

RosPublisherSystem uses separate Serializer interface

f84754c

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

RosSubscriberSystem uses separate Serializer interface

cd82726

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add RS-flip-flop example

e85f60d

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

sloretz self-assigned this Nov 14, 2020

sloretz commented Nov 14, 2020

View reviewed changes

drake_ros_systems/README.md Show resolved Hide resolved

Add get_type_support() to SerializerInterface

586100b

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

EricCousineau-TRI reviewed Nov 19, 2020

View reviewed changes

drake_ros_systems/include/drake_ros_systems/drake_ros.hpp Show resolved Hide resolved

sloretz added 8 commits December 2, 2020 11:10

Hello world pub example

1b012f2

Publisher and ROS interface working, subscriber not yet working. Signed-off-by: Shane Loretz <sloretz@openrobotics.org> Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Fix includes for DrakeRosInterface and RosInterfaceSystem

26661f0

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

ROS Subscriber works in Python

944b2e6

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Implement flip-flop example in Python

df079cf

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Update README for C++ or Python example

9283a5f

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Try more whitespace

3460dd2

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Bullet to numbered list

4ce1db9

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Commands to play with the example

8449db3

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

azeey approved these changes Dec 8, 2020

View reviewed changes

sloretz added 6 commits December 14, 2020 15:01

Blank line at EOF

ec6e3b9

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add linter tests

ae20bb1

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add LICENSE file and header comments

00da7c7

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Remove unused String import

fcd7fdd

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Satisfy uncrustify

8746fdf

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Satisfy cpplint

a04e038

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

sloretz added 7 commits December 14, 2020 17:14

Add additional publish triggers

30186b7

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Subscriber already unsubs on destruction

8fdf983

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Expose publish triggers to Python systems

5478e50

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Throw exception if Serialization fails

89b607a

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Remove obsolete comment

41d8f8a

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

EricCousineau-TRI reviewed Dec 17, 2020

View reviewed changes

sloretz added 7 commits December 17, 2020 15:35

NodeOptions defaults to global context

32b26bc

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add integration test

265e404

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add Python integration test

2776642

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Remove debug print

dac964b

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Fix bug where context wasn't being shut down

56bed38

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Add tests for DrakeRos

82ee8fb

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

Do more in Python

22880a8

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

sloretz commented Dec 18, 2020

View reviewed changes

sloretz merged commit 3a96616 into master Sep 8, 2021

sloretz deleted the generic_systems branch September 8, 2021 16:28

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Generic Drake/ROS systems + RS Flip Flop example #6

Generic Drake/ROS systems + RS Flip Flop example #6

sloretz commented Nov 14, 2020 •

edited

Loading

sloretz commented Nov 17, 2020

azeey left a comment •

edited

Loading

EricCousineau-TRI commented Dec 17, 2020

EricCousineau-TRI commented Dec 17, 2020

EricCousineau-TRI left a comment

sloretz left a comment

sloretz commented Dec 18, 2020 •

edited

Loading

sloretz commented Sep 8, 2021

Generic Drake/ROS systems + RS Flip Flop example #6

Generic Drake/ROS systems + RS Flip Flop example #6

Conversation

sloretz commented Nov 14, 2020 • edited Loading

RS Flip Flop example

Future enhancements

Code copying

sloretz commented Nov 17, 2020

azeey left a comment • edited Loading

Choose a reason for hiding this comment

EricCousineau-TRI commented Dec 17, 2020

EricCousineau-TRI commented Dec 17, 2020

EricCousineau-TRI left a comment

Choose a reason for hiding this comment

sloretz left a comment

Choose a reason for hiding this comment

sloretz commented Dec 18, 2020 • edited Loading

sloretz commented Sep 8, 2021

sloretz commented Nov 14, 2020 •

edited

Loading

azeey left a comment •

edited

Loading

sloretz commented Dec 18, 2020 •

edited

Loading